import threading

import pyrealsense2 as rs
import os
import struct
import serial
import time
import numpy as np
import threading as th
# 串口配置参考教程：https://blog.csdn.net/naruhina/article/details/105300566
# 硬件线材连接的时候务必须共地

# 连接
# GPIO14 -> TXD
# GPIO15 -> RXD


pipe = rs.pipeline()
cfg = rs.config()
#print(rs.__version__())
cfg.enable_stream(rs.stream.pose)
ctx = rs.context()
devices = ctx.query_devices()

uart = serial.Serial('/dev/ttyAMA0', 115200)
if not uart.isOpen:
    uart.open()
    print("Message: uart ready")
if uart.isOpen():
    print("message: uart has been opened")


if pipe.start(cfg):
    print("Message: T265 connected")
else:
    print("Error: conect error")
    exit()

def float_to_bytes(float_number):
    """
    浮点数转化为十六进制字符串
    :param float_number:
    :return:
    """
    return struct.pack('<f', float_number)

def bytes_to_float(hex_bytes):
    # 使用struct.unpack()函数将字节序列解析为浮点数
    float_val = struct.unpack('<f', hex_bytes)[0]
    return float_val


# 校验和
def xor_checksum(data):
    """
    校验和计算
    :param data: 数据字符串
    :return: 返回校验和字节
    """
    checksum = 0
    for byte in data:
        checksum ^= byte
    return checksum

def quaternion_to_euler_angle(qx, qy, qz, qw):
    # 计算欧拉角
    roll = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))
    pitch = np.arcsin(2 * (qw * qy - qz * qx))
    yaw = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))
    # 转换为度数
    roll = np.rad2deg(roll)
    pitch = np.rad2deg(pitch)
    yaw = np.rad2deg(yaw)

    return roll,pitch,yaw

last_px=0.0
last_py=0.0
last_pz=0.0
px=0.0
py=0.0
pz=0.0

def message_pack_pos(data):
    """
    位置数据打包函数
    :param data: 数据包
    :return:
    """
    global last_px
    global last_py
    global last_pz
    global px
    global py
    global pz
    start_byte = b'\x51'
    end_byte = b'\xff'
    type_byte = b'\x02'
    px = float_to_bytes(data.translation.x+last_px)
    py = float_to_bytes(data.translation.y+last_py)
    pz = float_to_bytes(data.translation.z+last_pz)
    print(px)
    data_w = float_to_bytes(0.00)
    length_byte = (len(px) + len(py) + len(pz) + len(data_w)).to_bytes(1, 'little')
    payload = type_byte + length_byte + px + py + pz + data_w
    checksum = xor_checksum(payload)
    package = start_byte + payload + bytes([checksum]) + end_byte
    return package


def message_pack(data, type):
    """
    将得到的位姿态信息打包为数据帧格式
    :param data: T265的位姿数据
    :param type: 数据类型
    :return: 打包好的数据帧
    """
    start_byte = b'\x51'
    end_byte = b'\xff'
    if type == 'pos':
        type_byte=b'\x02'
        # data_x = float_to_bytes(round(data.translation.x,2))
        # data_y = float_to_bytes(round(data.translation.y,2))
        # data_z = float_to_bytes(round(data.translation.z,2))
        #print(data_x)
    elif type == 'speed':
        type_byte = b'\x03'
        data_x = float_to_bytes(data.velocity.x)
        data_y = float_to_bytes(data.velocity.y)
        data_z = float_to_bytes(data.velocity.z)
    elif type == 'acc':
        type_byte = b'\x04'
        data_x = float_to_bytes(data.acceleration.x)
        data_y = float_to_bytes(data.acceleration.y)
        data_z = float_to_bytes(data.acceleration.z)
    else:
        print("data err")
        return 0
    data_w=float_to_bytes(0.00)
    length_byte = (len(data_x) + len(data_y) + len(data_z) + len(data_w)).to_bytes(1, 'little')
    payload = type_byte + length_byte + data_x + data_y + data_z + data_w
    checksum = xor_checksum(payload)
    package = start_byte + payload + bytes([checksum]) + end_byte
    return package

#发送四元数数据
def message_pack_rotation(data):
    """
    打包四元数数据
    :param data: T265上传的四元数数据
    :return: 打包好的数据帧
    """
    start_byte = b'\x51'
    type_byte = b'\x01'
    end_byte = b'\xff'

    a = quaternion_to_euler_angle(data.rotation.w, data.rotation.x, data.rotation.y, data.rotation.z)
    roll = round(a[0],2)
    yaw = round(a[1],2)
    pitch = round(a[2],2)
    x = float_to_bytes(roll)
    y = float_to_bytes(yaw)
    z = float_to_bytes(pitch)
    w = float_to_bytes(0)
    length_byte = (len(x) + len(y) + len(z) + len(w)).to_bytes(1, 'little')
    payload = type_byte + length_byte  + x + y + z + w
    checksum = xor_checksum(payload)
    package = start_byte + payload + bytes([checksum])+end_byte
    return package



if __name__ == "__main__":
    #send_pack_cmd('online')
    while True:
        try:
            frames = pipe.wait_for_frames()# 等待来自相机的下一帧数
            pose = frames.get_pose_frame()# 捕获位姿帧信息
            if pose:
                data = pose.get_pose_data()
                package_rotation = message_pack_rotation(data)
                uart.write(package_rotation)
                package_pos = message_pack_pos(data)
                uart.write(package_pos)
                package_speed = message_pack(data, 'speed')
                uart.write(package_speed)
                package_acc = message_pack(data, 'acc')
                uart.write(package_acc)
        except RuntimeError:
            print("Fxxk U Intel!!")
            last_px = bytes_to_float(px)
            last_py = bytes_to_float(py)
            last_pz = bytes_to_float(pz)
            pipe.stop()
            #time.sleep(1)
            pipe.start(cfg)
